#include <iostream>
#include <Eigen/Dense>
#include <fstream>
#include <string>
#include <cmath>
#include "angle.h"
#include "../include/class.h"
#include "../include/file.h"
#include "../include/initial.h"
using namespace std;
using namespace Eigen;

//////////////////////////////////////get truth state
State getState(string fileName){
    double data[10];
    ifstream inFileTrueValue(fileName, ios::in);

    if (!inFileTrueValue) {
        cout << "Can't open the file!" << endl;
    } else {
        cout << "Successfully open the file!" << endl;
    }
    inFileTrueValue.read((char *)data, 80);


    /*finally it to 弧度*/
    Vector3d attitude;
    attitude << data[7] * M_PI / 180, data[8] * M_PI / 180, data[9] * M_PI / 180;
    Vector3d v;
    v << data[4], data[5], data[6];
    Vector3d position;
    position << data[1] * M_PI / 180, data[2] * M_PI / 180, data[3];
    double time = data[0];

    State state(attitude, v, position, time);
    inFileTrueValue.close();
    return state;

}



/////////////////////////////////////getImuData
IMUDATA getImuData(double data[], int i){
    if (i == 0){//读取角度数据
        /*finally it to 弧度*/
        Vector3d gyroData(data[1] / 180 * M_PI, data[2] / 180 * M_PI, data[3] / 180 * M_PI);
        Vector3d accelData(data[4], data[5], data[6]);
        double time = data[0];
        IMUDATA imuData1(gyroData, accelData, time);
        return imuData1;
    }
    else{//读取弧度数据
        /*finally it to 弧度*/
        Vector3d gyroData(data[1], data[2], data[3]);
        Vector3d accelData(data[4], data[5], data[6]);
        double time = data[0];
        IMUDATA imuData1(gyroData, accelData, time);
        return imuData1;
    }

}






//////////////////////////////////////get ImuDataFromASC
//模板函数：将string类型变量转换为常用的数值类型（此方法具有普遍适用性）
template <class Type>
Type stringToNum(const string& str)
{
    istringstream iss(str);
    Type num;
    iss >> num;
    return num;
}

IMUDATA getImuDataFromASC(string imuALine, istream & inFile) {

    int *symbolPosition;//得到符号的位置
    int subStringNumber = 0;//d得到符号的数量

    //imu数据
    string sGyroX;
    string sGyroY;
    string sGyroZ;
    string sAccelX;
    string sAccelY;
    string sAccelZ;
    string sTime;

    double dGyroX;
    double dGyroY;
    double dGyroZ;
    double dAccelX;
    double dAccelY;
    double dAccelZ;

    double dGyroFront;
    double dGyroRight;
    double dGyroDown;
    double dAccelFront;
    double dAccelRight;
    double dAccelDown;
    double dTime;

    int m = 0;

    while (imuALine[0] != '%' ||//如果没有找到正确的数据，就一直找下去，直到文件结尾
           imuALine[1] != 'R' ||
           imuALine[2] != 'A' ||
           imuALine[3] != 'W' ||
           imuALine[4] != 'I' ||
           imuALine[5] != 'M' ||
           imuALine[6] != 'U' ||
           imuALine[7] != 'S' ||
           imuALine[8] != 'A'){

        if (!getline(inFile, imuALine)){
            break;
        }
    }

    if (imuALine.length() > 9) {//防止文件结尾后，imuALine里面仍然不是正确的数据
        if (imuALine[0] == '%' &&
            imuALine[1] == 'R' &&
            imuALine[2] == 'A' &&
            imuALine[3] == 'W' &&
            imuALine[4] == 'I' &&
            imuALine[5] == 'M' &&
            imuALine[6] == 'U' &&
            imuALine[7] == 'S' &&
            imuALine[8] == 'A') {

            // get 一列中有多少个符号
            for (int i = 0; i < imuALine.length(); i++) {
                if ((imuALine[i] == ',') || (imuALine[i] == '*') || (imuALine[i] == ';')) {
                    subStringNumber++;
                }
            }

            symbolPosition = new int[subStringNumber];//根据,;*数量动态创建一个长度的数组用于存储这些符号的位置

            //get这些符号的位置
            for (int i = 0; i < imuALine.length(); i++) {
                if ((imuALine[i] == ',') || (imuALine[i] == '*') || (imuALine[i] == ';')) {
                    symbolPosition[m] = i;
                    m++;
                }
            }

            //START CUT OFF
            //cout << imuALine << endl;
            sTime = imuALine.substr(symbolPosition[3] + 1, symbolPosition[4] - symbolPosition[3] - 1);
            sAccelZ = imuALine.substr(symbolPosition[5] + 1, symbolPosition[6] - symbolPosition[5] - 1);
            sAccelY = imuALine.substr(symbolPosition[6] + 1, symbolPosition[7] - symbolPosition[6] - 1);
            sAccelX = imuALine.substr(symbolPosition[7] + 1, symbolPosition[8] - symbolPosition[7] - 1);
            sGyroZ = imuALine.substr(symbolPosition[8] + 1, symbolPosition[9] - symbolPosition[8] - 1);
            sGyroY = imuALine.substr(symbolPosition[9] + 1, symbolPosition[10] - symbolPosition[9] - 1);
            sGyroX = imuALine.substr(symbolPosition[10] + 1, symbolPosition[11] - symbolPosition[10] - 1);
            delete[] symbolPosition;


            //数据格式转换，因为Y轴给出的反方向，所以要乘以 -1,而且陀螺仪的输出要变成弧度
            dTime = stringToNum<double>(sTime);
            dGyroX = stod(sGyroX) * 0.1 * pow(2, -8)  / 3600 * M_PI / 180;//* 1.0 / T;
            dGyroY = stod(sGyroY) * -1 * 0.1 * pow(2, -8)  / 3600 * M_PI / 180;// * 1.0 / T;
            dGyroZ = stod(sGyroZ) * 0.1 * pow(2, -8)  / 3600 * M_PI / 180;// * 1.0 / T;
            dAccelX = stod(sAccelX) * 0.05 * pow(2, -15);// * 1.0 / T;
            dAccelY = stod(sAccelY) * -1 * 0.05 * pow(2, -15);// * 1.0 / T;
            dAccelZ = stod(sAccelZ) * 0.05 * pow(2, -15);//* 1.0 / T;


            //坐标系的转换东北天，to 北东地
            //单位，陀螺仪rad，加速度计m/s

            //dTime = stod(sTime);也行，两种方式
            dGyroFront = dGyroY ;
            dGyroRight = dGyroX;
            dGyroDown = -1 * dGyroZ;
            dAccelFront = dAccelY;
            dAccelRight = dAccelX;
            dAccelDown = -1 * dAccelZ;

            Vector3d gyroData(dGyroFront, dGyroRight, dGyroDown);
            Vector3d accelData(dAccelFront, dAccelRight, dAccelDown);


            IMUDATA imuData1(gyroData, accelData, dTime);
            return imuData1;
        }

    }
}






//////////////////////////////////////////////////////////////////get gnssData
GNSSDATA getGNSSDATA(string gnssDataALine){

    string sGpsTime;
    string sLatitude;
    string sLongitude;
    string sHeight;
    string sLatitudeStd;
    string sLongitudeStd;
    string sHeightStd;

    double dGpsTime;
    double dLatitude;
    double dLongitude;
    double dHeight;
    double dLatitudeStd;
    double dLongitudeStd;
    double dHeightStd;

    int length = gnssDataALine.length();
    string subString[7];//一共有7个数据块
    int m = 0;
    int cutPosition1 = 0;
    int cutPosition2 = 0;
    while (gnssDataALine[cutPosition1] == ' '){//防止最开始的数据是‘ ’导致错误判断
        cutPosition1++;
    }
    for (int i = cutPosition1 + 1; i < length; i++){//从非空格的第二个数开始，避免出现访问内存超出的情况
        if (gnssDataALine[i - 1] == ' ' && gnssDataALine[i] != ' '){
            cutPosition2 = i;
            subString[m] = gnssDataALine.substr(cutPosition1, cutPosition2 - cutPosition1);
            cutPosition1 = i;
            m++;
        }
        if (i == gnssDataALine.length() - 1) {
            cutPosition2 = i;
            subString[m] = gnssDataALine.substr(cutPosition1, cutPosition2 - cutPosition1 + 1);
        }
    }

//    for (int i = 1; i < length - 1; i++){//从第一个开始，前一个结束避免出现访问内存超出的情况
//        if (gnssDataALine[i -1] != ' ' && gnssDataALine[i] == ' '){
//            cutPosition2 = i;
//            subString[m] = gnssDataALine.substr(cutPosition1, cutPosition2 - cutPosition1);
//            cutPosition1 = i;
//            m++;
//        }
//    }


    sGpsTime = subString[0];
    sLatitude = subString[1];
    sLongitude = subString[2];
    sHeight = subString[3];
    sLatitudeStd = subString[4];
    sLongitudeStd = subString[5];
    sHeightStd = subString[6];

    dGpsTime = stringToNum<double>(sGpsTime);
    dLatitude = stringToNum<double>(sLatitude) / 180 * M_PI;//deg to radian
    dLongitude = stringToNum<double>(sLongitude) / 180 * M_PI;//deg to radian
    dHeight = stringToNum<double>(sHeight);
    dLatitudeStd = stringToNum<double>(sLatitudeStd);
    dLongitudeStd = stringToNum<double>(sLongitudeStd);
    dHeightStd = stringToNum<double>(sHeightStd);

    GNSSDATA gnssData1(Vector3d(dLatitude, dLongitude, dHeight),
                       Vector3d (dLatitudeStd, dLongitudeStd,
                                 dHeightStd), dGpsTime);
    return gnssData1;
}


/////////////////////////////////////////out to text file
void outTextFile(ostream & outFile, State stateNow){
    outFile << 2017;
    outFile.precision(10);
    outFile.fill(' ');
    outFile.width(20);
    outFile << fixed << right << stateNow.getTime();
    outFile.precision(10);
    outFile.fill(' ');
    outFile.width(17);
    outFile << fixed << right << stateNow.getLatitude() * 180.0 / M_PI;
    outFile.precision(10);
    outFile.fill(' ');
    outFile.width(17);
    outFile << fixed << right << stateNow.getLongitude() * 180.0 / M_PI;
    outFile.precision(3);
    outFile.fill(' ');
    outFile.width(17);
    outFile << fixed << right << stateNow.getHeight();
    outFile.precision(3);
    outFile.fill(' ');
    outFile.width(11);
    outFile << fixed << right << stateNow.getVNorth();
    outFile.precision(3);
    outFile.fill(' ');
    outFile.width(11);
    outFile << fixed << right << stateNow.getVEast();
    outFile.precision(3);
    outFile.fill(' ');
    outFile.width(11);
    outFile << fixed << right << stateNow.getVDown();
    outFile.precision(8);
    outFile.fill(' ');
    outFile.width(14);
    outFile << fixed << right << stateNow.getRoll() * 180.0 / M_PI;
    outFile.precision(8);
    outFile.fill(' ');
    outFile.width(14);
    outFile << fixed << right << stateNow.getPitch() * 180.0 / M_PI;
    outFile.precision(8);
    outFile.fill(' ');
    outFile.width(14);
    outFile << fixed << right << stateNow.getHeading() * 180.0 / M_PI;
    outFile << endl;
}


//////////////////////////////////////////////out to binary file
void outBinaryFile(ostream & outFile, State stateNow){
    double dataOut[10];
    ////output data
    dataOut[0] = stateNow.getTime();
    dataOut[1] = stateNow.getLatitude() * 180 / M_PI;
    dataOut[2] = stateNow.getLongitude() * 180 / M_PI;
    dataOut[3] = stateNow.getHeight();
    dataOut[4] = stateNow.getVNorth();
    dataOut[5] = stateNow.getVEast();
    dataOut[6] = stateNow.getVDown();
    dataOut[7] = stateNow.getRoll() * 180 / M_PI;
    dataOut[8] = stateNow.getPitch() * 180 / M_PI;
    dataOut[9] = stateNow.getHeading() * 180 / M_PI;
    outFile.write((char *)dataOut, 80);
}

///////////////////////////out the imu data that been correct
void out_cor_imu(const IMUDATA& imuDataNow, ostream& file_out_imu_correct){
    file_out_imu_correct.precision(3);
    file_out_imu_correct.fill(' ');
    file_out_imu_correct.width(11);
    file_out_imu_correct << std::fixed << std::right << imuDataNow.getTime();
    file_out_imu_correct.precision(8);
    file_out_imu_correct.fill(' ');
    file_out_imu_correct.width(14);
    file_out_imu_correct << std::fixed << std::right << imuDataNow.getGyroFront();
    file_out_imu_correct.precision(8);
    file_out_imu_correct.fill(' ');
    file_out_imu_correct.width(14);
    file_out_imu_correct << std::fixed << std::right << imuDataNow.getGyroRight();
    file_out_imu_correct.precision(8);
    file_out_imu_correct.fill(' ');
    file_out_imu_correct.width(14);
    file_out_imu_correct << std::fixed << std::right << imuDataNow.getGyroDown();
    file_out_imu_correct.precision(8);
    file_out_imu_correct.fill(' ');
    file_out_imu_correct.width(14);
    file_out_imu_correct << std::fixed << std::right << imuDataNow.getAccelFront();
    file_out_imu_correct.precision(8);
    file_out_imu_correct.fill(' ');
    file_out_imu_correct.width(14);
    file_out_imu_correct << std::fixed << std::right << imuDataNow.getAccelRight();
    file_out_imu_correct.precision(8);
    file_out_imu_correct.fill(' ');
    file_out_imu_correct.width(14);
    file_out_imu_correct << std::fixed << std::right << imuDataNow.getAccelDown();
    file_out_imu_correct << std::endl;
}

void out_cor_imu_bin(const IMUDATA& imuDataNow, ostream& file_out_imu_correct_bin){
    double time = imuDataNow.getTime();
    double gyro_x = imuDataNow.getGyroFront();
    double gyro_y = imuDataNow.getGyroRight();
    double gyro_z = imuDataNow.getGyroDown();
    double accel_x = imuDataNow.getAccelFront();
    double accel_y = imuDataNow.getAccelRight();
    double accel_z = imuDataNow.getAccelDown();
    file_out_imu_correct_bin.write((char *)&time, sizeof(time));
    file_out_imu_correct_bin.write((char *)&gyro_x, sizeof(gyro_x));
    file_out_imu_correct_bin.write((char *)&gyro_y, sizeof(gyro_y));
    file_out_imu_correct_bin.write((char *)&gyro_z, sizeof(gyro_z));
    file_out_imu_correct_bin.write((char *)&accel_x, sizeof(accel_x));
    file_out_imu_correct_bin.write((char *)&accel_y, sizeof(accel_y));
    file_out_imu_correct_bin.write((char *)&accel_z, sizeof(accel_z));
}



////////////out the imu error 标准单位
void out_imu_error(const Vector<double, 24>& error_gyro_accel, double time, ostream& file_out_imu_error_bin){
    double error[24];
    for (int i = 0; i < 3; i++){
        error[i] = error_gyro_accel[i] * 3600.0 * 180.0 / M_PI;
    }
    for (int i =3; i < 6; i++){
        error[i] = error_gyro_accel[i] * 1e5;
    }
    for (int i = 6; i < 24; i++){
        error[i] = error_gyro_accel[i] * 1e6;
    }
    file_out_imu_error_bin.write((char *)&time, sizeof(time));
    for (int i = 0; i < 24; i++){
        file_out_imu_error_bin.write((char *)&error[i], sizeof(double));
    }
}

double result_a_line[10];

/////////out the sqrt（p）标准单位
void out_p(const Matrix<double, Dynamic, Dynamic>& p_kBefore, const IMUDATA& imuDataBefore, ostream& file_out_p){
    /*对switch_off中进行倒序，从小到大*/
    int switch_off1[24];
    int count = 0;
    for (int i = 0; i < 24; i++) {
        switch_off1[i] = -1;
    }
    for (int i = 0; i < 24; i++) {
        if (switch_off[i] == -1) {
            count = i;
            break;
        }
    }
    if (count != 0) {
        for (int i = 0; i < count; i++) {
            switch_off1[i] = switch_off[count - i - 1];
        }
    }

    /*初始化p阵*/
    Vector<double, 33> p = Vector<double, 33>::Zero();
    /*将p_kBefore中的数值复制到p阵中*/
    for (int i = 0; i < n; i++){
        p[i] = p_kBefore(i, i);
    }

    /*如果k出现在switch_off1中的话，代表这未被曾广进来的，因此需要从最后一个元素开始到第k个依次向后移位，然后在将delta_k[k]变成0*/
    for (int i = 0; i < 24; i++) {
        if (switch_off1[i] != -1) {
            for (int m = 32; m > switch_off1[i] + 9; m--) {
                p[m] = p[m - 1];
            }
            p[switch_off1[i]  + 9] = 0;//因为switch_off中没有包含前面的位置速度姿态误差，所以才要加上9
        }
        else {
            break;
        }
    }

    /*write*/
    double time = imuDataBefore.getTime();
    file_out_p.write((char *)&time, sizeof(time));
    for (int i = 0; i < 3; i++){
        p[i] = sqrt(p[i]);//m
    }
    for (int i = 3; i < 6; i++){
        p[i] = sqrt(p[i]);//m/s
    }
    for (int i = 6; i < 9; i++){
        p[i] = sqrt(p[i]);
        p[i] = p[i] * 180.0 / M_PI;//rad 2 deg
    }
    for (int i = 9; i < 12;i++){
        p[i] = sqrt(p[i]);
        p[i] = p[i] * 3600.0 * 180.0 / M_PI;//rad/s 2 deg/hr
    }
    for (int i = 12; i < 15; i++){
        p[i] = sqrt(p[i]);
        p[i] = p[i] * 1e5;//m/s^2 2 mGal
    }
    for (int i = 15; i < 33; i++){
        p[i] = sqrt(p[i]);
        p[i] = p[i] * 1e6;//1 2 PPM
    }
    for (int i = 0; i < 33; i++) {
        file_out_p.write((char *)&(p[i]), sizeof(p[i]));
    }
}
